/**
 * @author Ramon Loureiro
 * @version August 2010
 */


import lejos.nxt.*;

class THREADControladorMotores extends Thread {
   private Motor MotorTraccion ;
   private Motor MotorDireccion ;
   IntercambiadorDatos myDE ;
   private int DesiredPower ;
   private int actualPower ;

  public THREADControladorMotores (Motor MotorTraccion, Motor MotorDireccion,IntercambiadorDatos DE) {
      myDE = DE ;
      this.MotorTraccion = MotorTraccion;
      this.MotorTraccion.resetTachoCount();

      this.MotorDireccion = MotorDireccion;
      this.MotorDireccion.resetTachoCount();
  }



  public void run() {
    int ioIndex = 0;
    int i = 0 ;
    while (true) {
        try {
            this.DesiredPower = myDE.getPower();
            this.actualPower = this.MotorTraccion.getSpeed() ;
            if (this.MotorTraccion.isBackward()) this.actualPower = -this.actualPower;
            if (DesiredPower > actualPower) i = actualPower + 5 ;
            else if (DesiredPower < actualPower) i = actualPower - 5 ;
            else  i = DesiredPower ;

            this.MotorTraccion.setSpeed(i);
              if (i < 0) {
                 this.MotorTraccion.backward();
              } else if (i > 0) {
                 this.MotorTraccion.forward();
              } else {
                 this.MotorTraccion.stop();
              }

            Thread.sleep(25);
      } catch (InterruptedException ie) {

        }
    }
  }
}		